/*
 *  pointrsurface.h
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 17/03/11
 *	Modified by Roberto Ortega on 21/03/11
 *
 */

//#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET

#ifndef _PointRSurface_h_
#define _PointRSurface_h_

#include <fstream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
//#include "component.h"
#include "constraint.h"
#include "commonutil.h"
#include "JOINTS/joint.h"
#include "BODIES/body.h"
//#include "JOINTS/contact.h"

using namespace std;

class Surface;

class PointRSurface : public Constraint
{
public:
//	int numPhi;	// Number of constraints
	Joint* parentJoint;
	Surface* surface;	// Surface of body2

	Body* Body1;
	Body* Body2;
	
	vector<double> vLocalPoint;
	
/** Begin Local Data Members **************************************************/
	
	// Contact Coefficients
	double ex;
	double cs;
	double cd;
	double cf;

	//-- Bisection Method --//	
	double tole;
	int max;
	
	//-- Force Vector --//
	Eigen::VectorXd Fs; //Stifness	
	Eigen::VectorXd Fd; //Damping
	Eigen::VectorXd Ff; //Friction
	
	// Some Useful Vectors
	//vector<double> qBeta;
	Eigen::VectorXd qBeta;
	Eigen::VectorXd dV;
	Eigen::VectorXd dqm;
	
	// Global Position Vectors of Contact Point
	Eigen::Vector3d aG;
	Eigen::Vector3d bG;

	// Global Velocity Vectors of Contact Point
	Eigen::Vector3d daG;
	Eigen::Vector3d dbG;

	// Local Position Vectors of Contact Point
	Eigen::Vector3d aL;
	Eigen::Vector3d bL;
	
	// Globals Coordinates Vectors	
	Eigen::VectorXd qA; //Solid A
	Eigen::VectorXd qB; //Solid B

	// Globals Velocities Vectors	
	Eigen::VectorXd dqA; //Solid A
	Eigen::VectorXd dqB; //Solid B
	
	double S, S_old, DS;
	double V, V_old, DV;
	double G, dG;
	double beta;

	//Gradient of Surface (normal)
	Eigen::RowVector3d dS;
	
	Eigen::MatrixXd CA;
	Eigen::MatrixXd CB;
	
	Eigen::Matrix3d RB;
	Eigen::Vector3d e1B;
	Eigen::Vector3d e2B;
	Eigen::Vector3d e3B;
	
	Eigen::MatrixXd C0B;
	Eigen::MatrixXd C1B;
	Eigen::MatrixXd C2B;
	Eigen::MatrixXd C3B;
	
	Eigen::MatrixXd J;
	Eigen::MatrixXd D;
	
	//-- Tangent Matrix --//
	
	double ddG;
	
	MatrixXd ddS;
	MatrixXd ddV;

/** End Local Data Members ****************************************************/

/*	Data members from the base class Constraint
	int dim;					// Dimension
	int numDofs;				// Number of dofs
	vector<string> pointNames;						// Names of the points belonging to the Constraint
	map<string,Point*,less<string> > mPoints;		// Pointers to points belonging to the Constraint
	// WARNING: pointNames and mPoints have different ordering
	vector<int> localGlob;		// Local-global numbering
	double time;				// Time (local)
	double deltaT;
	double penalty;
	VectorXd q;
	VectorXd dq;
	VectorXd qMid;
	VectorXd qC;
	VectorXd dqC;
	VectorXd Fq;
	MatrixXd Kq;
	MatrixXd Kdq;
	double Phi;			// Constraint
	double PhiC;		// Coverged Constraint
	VectorXd dPhi;		// Constraint gradient
	MatrixXd ddPhi;		// Constraint Hessian
	VectorXd FqLag;
	VectorXd FqPenalty;
	VectorXd deltaQ;
	VectorXd discretedPhi;
	double perturbation;			// For numerical differentiation
	int integratorType;
	double lagMul;	// Lagrange multiplier
	bool augLagFlag, augConvergence;
	double FqRatio;
	bool isQuadratic;
	bool isContact;
	double midCorrection;
	double averagePhi;
	double denom;
	double midcorrection;
	vector<int> numDofsPoints;	
*/

public:
	PointRSurface();
	
	~PointRSurface(){}
	
	PointRSurface(vector<string>&, int, vector<double>&, Joint*);
	
	PointRSurface(vector<string>&, int, Joint*);
	
	/** Virtual functions from base */
	
	void calcPhi(const VectorXd &);
	void calcDPhi(const VectorXd &);
	void calcDDPhi(const VectorXd &);
	void setFqSymmetries();
	void setKqSymmetries();
	void calcFqC(const int&);
	void calcKqC(int);
	void calcKdqC(double&);
	void initializeConsResC(const bool&, const double&, const int&, const bool);
	void initializeConsTangC(const bool&);
	void updateCons();
	void calcEnerC(double& Ekin, double& Epot, double& Econ, double& Eamb, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in);

	
	// Local
	
//* Begin Local functions *****************************************************!
	
	void makeM(void);
	
	void computeForceStiffness(void);
	
	void computeForceDamping(void);
	
	void computeForceFriction(void);
	
	//void setCoordinates(vector<double>&);
	void setCoordinates(VectorXd&);
	
	//void calV(vector<double>&);
	void calV(VectorXd&);
	
	//void caldV(vector<double>&);
	void caldV(VectorXd&);

	//void calddV(vector<double>&);
	void calddV(VectorXd&);
	
	void calMatJ(void);
	
	double getGap(double);
	
	double getdGap(double);
	
	double getddGap(double);
	
	void caldVBeta(void);
	
	void calNewtonBeta(void);	
	
	double fBeta(double);

//*	Temporally functions ******************************************************!

	void calcLocalKqNumeric();

	Eigen::Matrix3d getR(Eigen::VectorXd&);
	
	Eigen::MatrixXd getC(Eigen::Vector3d, int size);

	void getGPos(Eigen::VectorXd&, Eigen::Vector3d&, Eigen::Vector3d&);
	
	void getGVel(Eigen::VectorXd&, Eigen::Vector3d&, Eigen::Vector3d&);
	
	void getLPos(Eigen::VectorXd&, Eigen::Vector3d&, Eigen::Vector3d&);

/** End Local functions *******************************************************/
	
	/** Friend functions */
	friend double lTwoNorm(const vector<double>& v1);
	friend double lTwoNorm(const vector<double>& v1, const vector<double>& v2);
	friend void uVersor(const vector<double>& v1, const vector<double>& v2, const double & dis, vector<double>& u);
	friend void printSTLVector(vector<double>& v);
	friend void printSTLMatrix(const vector< vector<double> >& m);
};

#endif
